#include "WPILib.h"
#include "Robot2489.h"

extern bool kicking;

void Robot2489::kickFunction()
{
	OpenLatch();
	Wait(.7);
	RetractKicker();
	Wait(.7);
	CloseLatch();
	Wait(0.01);
	kicking = false;
	OpenLatch();
	Wait( LATCHOPENTIME);
	RetractKicker();
	Wait( KICKERRETRACTTIME);
	CloseLatch();
	Wait( LATCHCLOSETIME);
	ChargeKicker();
	Wait( KICKERREADYINGTIME);
}

/**
 * ChargeKicker arms the kicker while the gate latch is still closed so that
 * the kick will be more powerful when it's released, because the kicker
 * will be trying to go with the bungie cords' force. This function is only
 * called when we want to charge the kicker for a STRONG kick.
 */
void Robot2489::ChargeKicker()
{
	// Arm the kicker.
	solKickIn->Set(0);
	solKickOut->Set(1);
}

/**
 * RelaxKicker makes the kicker solenoids both turn off, so that when the
 * gate latch releases it the kick will be a MEDIUM powered kick.
 */
void Robot2489::RelaxKicker()
{
	// Turn off both kickers.
	solKickIn->Set(0);
	solKickOut->Set(0);
}

/**
 * RetractKicker makes the kicker go back in after a kick, so we can kick
 * again.
 *
 * We also
 * use this function to anti-charge the kicker while the gate latch is still
 * closed so that the kick will be less powerful when it's released, because
 * the kicker will be actively resisting the pull of the bungie cords.
 * We then use this function to ANTI-charge the kicker for a WEAK kick.
 */
void Robot2489::RetractKicker()
{
	solKickIn->Set(1);
	solKickOut->Set(0);
}

/**
 * OpenLatch opens the gate latch so that the kicker kicks.
 */
void Robot2489::OpenLatch()
{
	solLatch->Set(1);
}

/**
 * CloseLatch closes the gate latch after the kicker retracts.
 */
void Robot2489::CloseLatch()
{
	solLatch->Set(0);
}
